
#include <AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
AF_DCMotor motor[4]={motor1,motor2,motor3,motor4};
#define MOTORNUM  4
//speed 小于120时，启动不了
void forward_drive(int speed)
{
 
 for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(FORWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }
}
void backward_drive(int speed)
{
for(char i=0;i<MOTORNUM;i++)
  {
     motor[i].run(BACKWARD);
      motor[i].setSpeed(speed); 
   // motor[i].run(RELEASE);

  }

}
void rotate(int rot)
{

  
}
void stop()
{
  for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(0); 
    motor[i].run(RELEASE);

  }

}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);           // set up Serial library at 9600 bps
  Serial.println("Motor test!");

  // turn on motor
 for(char i=0;i<MOTORNUM;i++)
  {
    motor[i].setSpeed(200); 
    motor[i].run(RELEASE);

  }

 
 
}

void loop() {
  // put your main code here, to run repeatedly:
  //step1 .forward 1m
  // forward_drive(200);
  // delay(1000);
  //  forward_drive(100);
  //   delay(1000);
    forward_drive(120);
    delay(100);
    forward_drive(80);
    delay(3000);
    stop();
    /*
    delay(3000);
    backward_drive(120);
    delay(100);
    backward_drive(80);
    delay(3000);
 stop();
 */    
    while(1)
    {
     

    }

  //step2. back 1m

}
